package br.com.frbb.ugvbr.robot;

import br.com.frbb.ugvbr.accessory.USBAccessoryManager;

public class Motor {

	private int forwardCommand;
	private int stopForwardCommand;
	private int backwardCommand;
	private int stopBackwardCommand;
	
	Motor(int forwardCommand, int stopForwardCommand, int backwardCommand, int stopBackwardCommand) {
		this.forwardCommand = forwardCommand;
		this.stopForwardCommand = stopForwardCommand;
		this.backwardCommand = backwardCommand;
		this.stopBackwardCommand = stopBackwardCommand;
	}
	
	public void move(int speed) {
		
		stop();
		
		if(speed >= 0) {
			sendCommand(forwardCommand, speed);
		}
		else {
			sendCommand(backwardCommand, speed * -1);
		}
	}
	
	public void stop() {
		sendCommand(stopForwardCommand, 0);
		sendCommand(stopBackwardCommand, 0);
	}
	
	private void sendCommand(int command, int speed) {
		byte[] message = new byte[2];
		
		message[0] = (byte)command;
		message[1] = (byte)speed;
		
		USBAccessoryManager.sendMessage(message);
	}
}
